#include "L3G4200D.h"
#include "math.h"
#include <stdio.h>
#include <stdlib.h>

#define L3G4200D_ADDRESS (0xD2 >> 1  )

// register addresses
#define L3G4200D_WHO_AM_I      0x0F

#define L3G4200D_CTRL_REG1     0x20
#define L3G4200D_CTRL_REG2     0x21
#define L3G4200D_CTRL_REG3     0x22
#define L3G4200D_CTRL_REG4     0x23
#define L3G4200D_CTRL_REG5     0x24
#define L3G4200D_REFERENCE     0x25
#define L3G4200D_OUT_TEMP      0x26
#define L3G4200D_STATUS_REG    0x27

#define L3G4200D_OUT_X_L       0x28
#define L3G4200D_OUT_X_H       0x29
#define L3G4200D_OUT_Y_L       0x2A
#define L3G4200D_OUT_Y_H       0x2B
#define L3G4200D_OUT_Z_L       0x2C
#define L3G4200D_OUT_Z_H       0x2D

#define L3G4200D_FIFO_CTRL_REG 0x2E
#define L3G4200D_FIFO_SRC_REG  0x2F

#define L3G4200D_INT1_CFG      0x30
#define L3G4200D_INT1_SRC      0x31
#define L3G4200D_INT1_THS_XH   0x32
#define L3G4200D_INT1_THS_XL   0x33
#define L3G4200D_INT1_THS_YH   0x34
#define L3G4200D_INT1_THS_YL   0x35
#define L3G4200D_INT1_THS_ZH   0x36
#define L3G4200D_INT1_THS_ZL   0x37
#define L3G4200D_INT1_DURATION 0x38

//#define GYR_ADDRESS (0x3C >> 1  )


L3G4200D::L3G4200D(int file) : I2CDevice(file, L3G4200D_ADDRESS)
{
  writeByte(L3G4200D_CTRL_REG1, 0x4F);
	// 0x0F = 0b01001111
  // data rate 200Hz
	// Normal power mode, all axes enabled

  writeByte(L3G4200D_CTRL_REG4, 0x00); //high res
}



// Reads the 3 gyro channels and stores them in vector g
void L3G4200D::read()
{
  __u8 buf[6];
  readBlock( L3G4200D_OUT_X_L | (1 << 7), buf, 6);


  for (int i = 0; i < 6; i++)
  {
    buf[i] = ~buf[i];
    buf[i] += 0x01;
  }
  /*
  printf("g: ");
  for (int i = 0; i < 6; i++)
    printf("%02x ", buf[i]);
  printf("\n");
  */

  __u8 xla = buf[0];
  __u8 xha = buf[1];
  __u8 yla = buf[2];
  __u8 yha = buf[3];
  __u8 zla = buf[4];
  __u8 zha = buf[5];

  __s16 x = xha << 8 | xla;
  __s16 y = yha << 8 | yla;
  __s16 z = zha << 8 | zla;

	g.x() = x;
	g.y() = y;
	g.z() = z;
}

